'''
1、接受tessng向kafka发送的车辆数据,并进行展示


'''
import copy
import os
from queue import Queue
from concurrent.futures import ThreadPoolExecutor
import open3d as o3d
import carla
import numpy as np
import time
import redis
from carla import Transform, Rotation, Location
from kafka import KafkaConsumer, KafkaProducer
import json
from pyproj import Proj
import cv2


class myCarla:
    def __init__(self, carla_ip, carla_port):

        # ==================carla初始化======================
        # 创建carla客户端
        self.client = carla.Client(carla_ip, carla_port)
        self.client.set_timeout(10.0)
        self.world = self.client.load_world('tongji')
        self.settings = self.world.get_settings()
        self.blueprint_library = self.world.get_blueprint_library()
        # 获取carla中的交通管理者
        self.traffic_manager = self.client.get_trafficmanager(8000)
        # ==================基本信息======================
        # 转换坐标
        self.rule = Proj("+proj=tmerc +lon_0=121.2092870660126 +lat_0=31.292829882838856 +ellps=WGS84")
        # ==================缓存======================
        # carla中运行的主车，key-已经生成的车辆id，value-carla中id
        self.run_bjc_carla_id_dict = {}
        # carla中运行的背景车，key-已经生成的车辆id，value-carla中id
        self.run_target_carla_id_dict = {}
        # 车辆类型和carla类型字典关系，key-车辆类型，value-carla类型
        self.now_car_type_carla_type_dist = {
            1: 'vehicle.lincoln.mkz_2017',
            2: 'vehicle.bmw.grandtourer'
        }
        # 默认车辆类型
        self.default_carla_type = 'vehicle.lincoln.mkz_2017'
        # ==================雷达点云、相机======================
        # 【相机图片、雷达点云】数据保存路径
        self.root_path = r'D:\hsy\data'
        # 存放相机数据的队列
        self.png_queue = Queue()
        # 存放雷达点云数据的队列
        self.point_cloud_queue = Queue()
        # 存放 绑定后的雷达
        self.lidar_list = []
        # 存放 绑定后的相机
        self.camera_list = []
        # 雷达点云展示
        self.lidar_point_cloud_show = True
        # 雷达点云存储
        self.lidar_point_cloud_save = False
        # 相机图片展示
        self.camera_image_show = True
        # 相机图片保存
        self.camera_image_save = False

        self.to_reset_view_point = True
        self.vis = None
        # ==================redis======================
        self.redis_host = '10.102.1.113'
        self.redis_port = 6379
        self.redis_password = 'Wanji@300552!'
        self.redis_client = redis.Redis(host=self.redis_host, port=self.redis_port, password=self.redis_password, db=0)

    # 设置天气
    def set_weather(self):
        weather = carla.WeatherParameters(
            cloudiness=10.0,  # 0-100  0 是晴朗的天空，100 是完全阴天
            precipitation=0.0,  # 0 表示没有下雨，100 表示大雨
            precipitation_deposits=0.0,  # 0 表示道路上没有水坑，100 表示道路完全被雨水覆盖
            wind_intensity=0.0,  # 0 表示平静，100 表示强风，风会影响雨向和树叶
            sun_azimuth_angle=0.0,  # 太阳方位角，0～360
            sun_altitude_angle=90.0,  # 太阳高度角，90 是中午，-90 是午夜
            fog_density=0.0,  # 0～100 表示雾的浓度或厚度，仅影响RGB相机传感器
            fog_distance=0.0,  # 雾开始的距离，单位为米
            wetness=0.0,  # 0～100 表示道路湿度百分比，仅影响RGB相机传感器
            fog_falloff=0.0,  # 雾的密度，0至无穷大，0 表示雾比空气轻，覆盖整个场景，1表示与空气一样，覆盖正常大小的建筑物
            scattering_intensity=0.0,  # 控制光线对雾的穿透程度
            mie_scattering_scale=0.0,  # 控制光线与花粉或空气等大颗粒的相互作用，导致天气朦胧，光源周围有光晕，0表示无影响
            rayleigh_scattering_scale=0.0331,  # 控制光与空气分子等小粒子的相互作用，取决于光波长，导致白天蓝天或晚上红天
        )
        self.world.set_weather(weather)

    # 设置视角坐标
    def set_view(self, lon, lat):
        x, y = self.rule(lon, lat)
        z = 130
        view_angle = Transform(Location(x=x, y=-y, z=z),
                               Rotation(pitch=-90, yaw=-180, roll=45.00000))
        self.world.get_spectator().set_transform(view_angle)  # 设置世界视角

    # 创建交通管理器对象
    def set_traffic_manager(self):
        self.traffic_manager.set_global_distance_to_leading_vehicle(2.0)
        self.traffic_manager.global_percentage_speed_difference(40.0)
        self.traffic_manager.set_synchronous_mode(True)
        if not self.settings.synchronous_mode:
            print("开启异步模式")
            synchronous_master = True
            self.settings.synchronous_mode = False
            self.settings.fixed_delta_seconds = 0.1
            self.world.apply_settings(self.settings)

    # 获取carla车辆
    def get_carla_car_type(self, veh_type):
        if veh_type in self.now_car_type_carla_type_dist.keys():
            carla_type = self.now_car_type_carla_type_dist[veh_type]
        else:
            carla_type = self.default_carla_type
        carla_car = self.world.get_blueprint_library().find(carla_type)
        return carla_car

    # 获取carla车辆信息
    @staticmethod
    def get_vehicle_info(vehicle):
        transform = vehicle.get_transform()
        velocity = vehicle.get_velocity()
        return {
            'id': vehicle.id,
            'type_id': vehicle.type_id,
            'location': {
                'x': transform.location.x,
                'y': transform.location.y,
                'z': transform.location.z
            },
            'rotation': {
                'pitch': transform.rotation.pitch,
                'yaw': transform.rotation.yaw,
                'roll': transform.rotation.roll
            },
            'velocity': {
                'x': velocity.x,
                'y': velocity.y,
                'z': velocity.z
            }
        }

    # 通过redis，获取主车信息并设置
    def set_target_car_for_redis(self, redis_channel):
        try:
            pubsub = self.redis_client.pubsub()
            pubsub.subscribe(redis_channel)
            for message in pubsub.listen():
                if message['type'] == 'message':
                    # 接收到数据，添加处理逻辑
                    receive_data = message['data'].decode('utf-8')
                    data = json.loads(receive_data)
                    participants = data['value']['value']
                    # 当前帧创建失败的车辆id集合
                    now_error_id_list = []
                    # 当前帧所有车辆id集合
                    now_id_list = []
                    for p in participants:
                        now_id_list.append(p["id"])
                    # 添加交通流
                    for par in participants:
                        x, y = self.rule(par["longitude"], par["latitude"])
                        veh_type = par['vehicleType']
                        car_id = par['id']
                        angle = par["courseAngle"]
                        # 判断是否已经生成过该车辆
                        if car_id not in self.run_target_carla_id_dict.keys():
                            # 没有生成过，则生成
                            trans = Transform(carla.Location(x=x, y=-y, z=15), carla.Rotation(yaw=angle - 90))
                            # 根据车辆类型生成车辆
                            carla_car = self.get_carla_car_type(veh_type)
                            # 将车辆生成请求添加到批量请求中
                            batch = [
                                carla.command.SpawnActor(carla_car, trans).then(
                                    carla.command.SetSimulatePhysics(carla.command.FutureActor, False))
                            ]
                            # 将车辆生成请求发送到服务器端，并获取响应
                            response = self.client.apply_batch_sync(batch, False)[0]
                            if response.error:
                                now_error_id_list.append(car_id)
                            else:
                                actor_id = response.actor_id
                                self.run_target_carla_id_dict[car_id] = actor_id
                                # 获取生成的车辆对象
                                vehicle = self.world.get_actor(actor_id)
                                # 在主车上设置相机
                                self.set_camera(vehicle)
                                # 在主车上设置雷达
                                self.set_lidar_64(vehicle)
                        else:
                            # 生成过，则更新位置和角度
                            # 更新位置和角度
                            trans = Transform(carla.Location(x=x, y=-y, z=15),
                                              carla.Rotation(yaw=par["courseAngle"] - 90))
                            carla_id = self.run_target_carla_id_dict[car_id]
                            carla_car = self.world.get_actor(carla_id)
                            if carla_car is not None:
                                carla_car.set_transform(trans)
                    # 合并、保存、展示点云
                    self.merge_save_show_lidar_data(self.root_path)
                    self.save_show_camera_image()
                    # print(f'【主车】个数：{len(self.run_target_carla_id_dict)}')
                    # 销毁不在当前帧的id
                    destroy_id_list = [id for id in self.run_target_carla_id_dict if id not in now_id_list]
                    for destroy_id in destroy_id_list:
                        carla_id = self.run_target_carla_id_dict[destroy_id]
                        vehicle = self.world.get_actor(carla_id)
                        if vehicle is not None:
                            vehicle.destroy()
                        del self.run_target_carla_id_dict[destroy_id]
                    # print(f'【主车】本次更新数量：{len(now_id_list)}，销毁数量：{len(destroy_id_list)}')
        finally:
            for cam in self.camera_list:
                cam.destroy()
            for lidar in self.lidar_list:
                lidar.destroy()

    # 通过redis，获取背景车信息并设置
    def set_bj_car_for_redis(self, redis_channel):
        pubsub = self.redis_client.pubsub()
        pubsub.subscribe(redis_channel)
        for message in pubsub.listen():
            if message['type'] == 'message':
                # 接收到数据，添加处理逻辑
                receive_data = message['data'].decode('utf-8')
                data = json.loads(receive_data)
                participants = data['value']['value']
                # 当前帧创建失败的车辆id集合
                now_error_id_list = []
                # 当前帧所有车辆id集合
                now_id_list = []
                for p in participants:
                    now_id_list.append(p["id"])
                # 添加交通流
                for par in participants:
                    x, y = self.rule(par["longitude"], par["latitude"])
                    veh_type = par['vehicleType']
                    car_id = par['id']
                    angle = par["courseAngle"]
                    # 判断是否已经生成过该车辆
                    if car_id not in self.run_bjc_carla_id_dict.keys():
                        # 没有生成过，则生成
                        trans = Transform(carla.Location(x=x, y=-y, z=15), carla.Rotation(yaw=angle - 90))
                        # 根据车辆类型生成车辆
                        carla_car = self.get_carla_car_type(veh_type)
                        # 将车辆生成请求添加到批量请求中
                        batch = [
                            carla.command.SpawnActor(carla_car, trans).then(
                                carla.command.SetSimulatePhysics(carla.command.FutureActor, False))
                        ]
                        # 将车辆生成请求发送到服务器端，并获取响应
                        response = self.client.apply_batch_sync(batch, False)[0]
                        if response.error:
                            now_error_id_list.append(car_id)
                        else:
                            self.run_bjc_carla_id_dict[car_id] = response.actor_id
                    else:
                        # 生成过，则更新位置和角度
                        # 更新位置和角度
                        trans = Transform(carla.Location(x=x, y=-y, z=15),
                                          carla.Rotation(yaw=par["courseAngle"] - 90))
                        carla_id = self.run_bjc_carla_id_dict[car_id]
                        carla_car = self.world.get_actor(carla_id)
                        if carla_car is not None:
                            carla_car.set_transform(trans)
                # print(f'【背景车】个数：{len(self.run_bjc_carla_id_dict)}')
                # 销毁不在当前帧的id
                destroy_id_list = [id for id in self.run_bjc_carla_id_dict if id not in now_id_list]
                for destroy_id in destroy_id_list:
                    carla_id = self.run_bjc_carla_id_dict[destroy_id]
                    vehicle = self.world.get_actor(carla_id)
                    if vehicle is not None:
                        vehicle.destroy()
                    del self.run_bjc_carla_id_dict[destroy_id]
                # print(f'【背景车】本次更新数量：{len(now_id_list)}，销毁数量：{len(destroy_id_list)}')

    # 发送数据到redis
    def send_data_for_redis(self, send_channel):
        while True:
            # 获取场景中的所有车辆
            carla_veh_list = self.world.get_actors().filter('vehicle.*')
            vehicle_list = []
            # 遍历所有车辆并获取信息
            for vehicle in carla_veh_list:
                vehicle_info = self.get_vehicle_info(vehicle)
                vehicle_list.append(vehicle_info)
            self.redis_client.publish(send_channel, json.dumps(vehicle_list))
            # 休眠一段时间，避免频繁发送
            time.sleep(0.1)

    # =========================================雷达=======================================================
    # 获取时间戳
    def get_time_stamp(self, ct):
        local_time = time.localtime(ct)
        data_head = time.strftime("%Y-%m-%d %H-%M-%S", local_time).split(' ')[-1]
        data_secs = (ct - int(ct)) * 1000
        time_stamp = "%s-%03d" % (data_head, data_secs)
        return time_stamp

    # 将雷达的点云保存到队列
    def sensor_callback(self, sensor_data, sensor_queue, sensor_name):
        sensor_queue.put((sensor_data.frame, sensor_data.timestamp, sensor_name, sensor_data))

    def _parse_lidar_cb(self, lidar_data):
        points = np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4'))
        points = copy.deepcopy(points)
        points = np.reshape(points, (int(points.shape[0] / 4), 4))
        return points

    # 保存点云数据
    def points2pcd(self, PCD_FILE_PATH, points):

        if os.path.exists(PCD_FILE_PATH):
            os.remove(PCD_FILE_PATH)
        # 写文件句柄
        handle = open(PCD_FILE_PATH, 'a')
        # 得到点云点数
        point_num = points.shape[0]
        # pcd头部（重要）
        handle.write(
            '# .PCD v0.7 - Point Cloud Data file format\nVERSION 0.7\nFIELDS x y z intensity\nSIZE 4 4 4 4\nTYPE F F F F\nCOUNT 1 1 1 1')
        string = '\nWIDTH ' + str(point_num)
        handle.write(string)
        handle.write('\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0')
        string = '\nPOINTS ' + str(point_num)
        handle.write(string)
        handle.write('\nDATA ascii')

        # 依次写入点
        for i in range(point_num):
            string = '\n' + str(points[i, 0]) + ' ' + str(points[i, 1]) + ' ' + str(points[i, 2]) + ' ' + str(
                points[i, 3])
            handle.write(string)
        handle.close()

    def set_lidar_64(self, carla_car):
        # 获取carla中的雷达
        lidar_bp_2_1 = self.blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_2_2 = self.blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_2_3 = self.blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_4_1 = self.blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_4_2 = self.blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_4_3 = self.blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_6_1 = self.blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_6_2 = self.blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_6_3 = self.blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_28 = self.blueprint_library.find('sensor.lidar.ray_cast')

        # 设置传感器公共参数
        atmosphere_attenuation_rate = '0.004'
        dropoff_general_rate = '0.0'
        dropoff_intensity_limit = '0.0'
        dropoff_zero_intensity = '0.0'
        noise_seed = '0.0'
        noise_stddev = '0.0'
        lidar_bp_list = [lidar_bp_2_1, lidar_bp_2_2, lidar_bp_2_3, lidar_bp_4_1, lidar_bp_4_2, lidar_bp_4_3,
                         lidar_bp_6_1,
                         lidar_bp_6_2, lidar_bp_6_3, lidar_bp_28]
        for lidar in lidar_bp_list:
            lidar.set_attribute('range', '200')
            lidar.set_attribute('rotation_frequency', str(int(1 / self.settings.fixed_delta_seconds)))
            lidar.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
            lidar.set_attribute('dropoff_general_rate', dropoff_general_rate)
            lidar.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
            lidar.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
            lidar.set_attribute('noise_seed', noise_seed)
            lidar.set_attribute('noise_stddev', noise_stddev)
        # 单独设置各雷达参数
        lidar_bp_2_1.set_attribute('channels', '2')
        lidar_bp_2_1.set_attribute('upper_fov', '-1.5')
        lidar_bp_2_1.set_attribute('lower_fov', '-1.7')
        lidar_bp_2_1.set_attribute('points_per_second', '36000')
        lidar_bp_28.set_attribute('channels', '28')
        lidar_bp_28.set_attribute('upper_fov', '-1.9')
        lidar_bp_28.set_attribute('lower_fov', '-4.6')
        lidar_bp_28.set_attribute('points_per_second', '504000')
        lidar_bp_6_1.set_attribute('channels', '6')
        lidar_bp_6_1.set_attribute('upper_fov', '-4.8')
        lidar_bp_6_1.set_attribute('lower_fov', '-5.8')
        lidar_bp_6_1.set_attribute('points_per_second', '108000')
        lidar_bp_4_1.set_attribute('channels', '4')
        lidar_bp_4_1.set_attribute('upper_fov', '-6.1')
        lidar_bp_4_1.set_attribute('lower_fov', '-7')
        lidar_bp_4_1.set_attribute('points_per_second', '72000')
        lidar_bp_4_2.set_attribute('channels', '4')
        lidar_bp_4_2.set_attribute('upper_fov', '-7.4')
        lidar_bp_4_2.set_attribute('lower_fov', '-8.6')
        lidar_bp_4_2.set_attribute('points_per_second', '72000')
        lidar_bp_4_3.set_attribute('channels', '4')
        lidar_bp_4_3.set_attribute('upper_fov', '-9.2')
        lidar_bp_4_3.set_attribute('lower_fov', '-11')
        lidar_bp_4_3.set_attribute('points_per_second', '72000')
        lidar_bp_6_2.set_attribute('channels', '6')
        lidar_bp_6_2.set_attribute('upper_fov', '-12')
        lidar_bp_6_2.set_attribute('lower_fov', '-17')
        lidar_bp_6_2.set_attribute('points_per_second', '108000')
        lidar_bp_6_3.set_attribute('channels', '6')
        lidar_bp_6_3.set_attribute('upper_fov', '-19')
        lidar_bp_6_3.set_attribute('lower_fov', '-29')
        lidar_bp_6_3.set_attribute('points_per_second', '108000')
        lidar_bp_2_2.set_attribute('channels', '2')
        lidar_bp_2_2.set_attribute('upper_fov', '-32')
        lidar_bp_2_2.set_attribute('lower_fov', '-35')
        lidar_bp_2_2.set_attribute('points_per_second', '36000')
        lidar_bp_2_3.set_attribute('channels', '2')
        lidar_bp_2_3.set_attribute('upper_fov', '-38')
        lidar_bp_2_3.set_attribute('lower_fov', '-42')
        lidar_bp_2_3.set_attribute('points_per_second', '36000')
        # 将雷达绑定到主车上
        vehicle_trans_lidar = Transform(Location(x=0, z=45), Rotation(yaw=347.65 - 90))
        lidar_2_1 = self.world.spawn_actor(lidar_bp_2_1, vehicle_trans_lidar, attach_to=carla_car)
        lidar_2_2 = self.world.spawn_actor(lidar_bp_2_2, vehicle_trans_lidar, attach_to=carla_car)
        lidar_2_3 = self.world.spawn_actor(lidar_bp_2_3, vehicle_trans_lidar, attach_to=carla_car)
        lidar_4_1 = self.world.spawn_actor(lidar_bp_4_1, vehicle_trans_lidar, attach_to=carla_car)
        lidar_4_2 = self.world.spawn_actor(lidar_bp_4_2, vehicle_trans_lidar, attach_to=carla_car)
        lidar_4_3 = self.world.spawn_actor(lidar_bp_4_3, vehicle_trans_lidar, attach_to=carla_car)
        lidar_6_1 = self.world.spawn_actor(lidar_bp_6_1, vehicle_trans_lidar, attach_to=carla_car)
        lidar_6_2 = self.world.spawn_actor(lidar_bp_6_2, vehicle_trans_lidar, attach_to=carla_car)
        lidar_6_3 = self.world.spawn_actor(lidar_bp_6_3, vehicle_trans_lidar, attach_to=carla_car)
        lidar_28 = self.world.spawn_actor(lidar_bp_28, vehicle_trans_lidar, attach_to=carla_car)
        # 监听雷达数据，并存储到队列中
        lidar_2_1.listen(lambda data: self.sensor_callback(data, self.point_cloud_queue, "lidar_2_1"))
        lidar_2_2.listen(lambda data: self.sensor_callback(data, self.point_cloud_queue, "lidar_2_2"))
        lidar_2_3.listen(lambda data: self.sensor_callback(data, self.point_cloud_queue, "lidar_2_3"))
        lidar_4_1.listen(lambda data: self.sensor_callback(data, self.point_cloud_queue, "lidar_4_1"))
        lidar_4_2.listen(lambda data: self.sensor_callback(data, self.point_cloud_queue, "lidar_4_2"))
        lidar_4_3.listen(lambda data: self.sensor_callback(data, self.point_cloud_queue, "lidar_4_3"))
        lidar_6_1.listen(lambda data: self.sensor_callback(data, self.point_cloud_queue, "lidar_6_1"))
        lidar_6_2.listen(lambda data: self.sensor_callback(data, self.point_cloud_queue, "lidar_6_2"))
        lidar_6_3.listen(lambda data: self.sensor_callback(data, self.point_cloud_queue, "lidar_6_3"))
        lidar_28.listen(lambda data: self.sensor_callback(data, self.point_cloud_queue, "lidar_28"))
        # 将绑定后的雷达放入集合中
        self.lidar_list.append(lidar_2_1)
        self.lidar_list.append(lidar_2_2)
        self.lidar_list.append(lidar_2_3)
        self.lidar_list.append(lidar_4_1)
        self.lidar_list.append(lidar_4_2)
        self.lidar_list.append(lidar_4_3)
        self.lidar_list.append(lidar_6_1)
        self.lidar_list.append(lidar_6_2)
        self.lidar_list.append(lidar_6_3)
        self.lidar_list.append(lidar_28)
        if self.lidar_point_cloud_show:
            self.point_cloud = o3d.geometry.PointCloud()
            self.vis = o3d.visualization.Visualizer()
            self.vis.create_window(window_name='Wanji 64line Lidar', width=600, height=500, left=0, top=500,
                                   visible=True)
            ctr = self.vis.get_view_control()
            self.vis.add_geometry(self.point_cloud)
            extrinsic = np.eye(4)
            W = 600
            H = 500
            camera_angle_x = 0.8457078337669373
            focal = 0.5 * W / np.tan(0.5 * camera_angle_x)
            # cx=(W-1)/2而不是W/2 !!!!!
            intrinsic = o3d.cuda.pybind.camera.PinholeCameraIntrinsic(width=W, height=H, fx=focal, fy=focal,
                                                                      cx=(W - 1) / 2,
                                                                      cy=(H - 1) / 2)
            param = o3d.cuda.pybind.camera.PinholeCameraParameters()
            param.extrinsic = extrinsic
            param.intrinsic = intrinsic
            ctr.convert_from_pinhole_camera_parameters(param)
            render_option = self.vis.get_render_option()
            render_option.point_size = 1.0
            render_option.background_color = np.asarray([0, 0, 0])  # 颜色 0为黑；1为白
            self.to_reset_view_point = True

    # 合并点云数据并保存
    def merge_save_show_lidar_data(self, save_path):
        # 将所有雷达的数据合并到一个点云文集中
        splicing = []
        p_timestamp = self.world.get_snapshot().timestamp.platform_timestamp
        w_timestamp = self.get_time_stamp(p_timestamp)
        # 将所有雷达的数据合并到一个点云文集中
        for i in range(0, len(self.lidar_list)):
            if not self.point_cloud_queue.empty():
                s_frame, s_timestamp, s_name, s_data = self.point_cloud_queue.get(True, 1.0)
                splicing.append(self._parse_lidar_cb(s_data))
        if splicing:
            concat_points = np.concatenate(splicing, axis=0)
            concat_points[:, 1] = [-p for p in concat_points[:, 1]]  # 将生成的点云Y值进行反转 由于carla的坐标系中Y值是反的
            # 点云存储
            if self.lidar_point_cloud_save:
                path = os.path.join(save_path, 'pcd')
                if not os.path.exists(path):
                    os.makedirs(path)
                pcd_path = path + "/" + str(w_timestamp) + ".pcd"
                self.points2pcd(pcd_path, concat_points)
            if self.lidar_point_cloud_show:
                points1 = np.array(concat_points)[:, :3]
                self.point_cloud.points = o3d.utility.Vector3dVector(points1)
                points_intensity = np.array(concat_points)[:, 3]  # intensity
                colors = []
                for num in points_intensity:
                    # print(num)
                    intensity = num * 255
                    if intensity <= 33:
                        r = 0
                        b = 255
                        g = int(7.727 * intensity)
                    elif intensity > 33 and intensity <= 66:
                        r = 0
                        b = int(255 - 7.727 * (intensity - 34))
                        g = 255
                    elif intensity > 66 and intensity <= 100:
                        r = int(7.727 * (intensity - 67))
                        b = 0
                        g = 255
                    elif intensity > 100 and intensity <= 255:
                        r = 255
                        b = 0
                        g = int(255 - 7.727 * (intensity - 100) / 4.697)
                    points_color = [r, g, b]
                    colors.append(points_color)
                # 根据 intensity 为点云着色，由于变化不大，这里设置为蓝色[0,0,0.8]
                self.point_cloud.colors = o3d.utility.Vector3dVector(colors)
                render_opt = self.vis.get_render_option()
                render_opt.point_size = 1
                render_opt.background_color = [0, 0, 0]

                box = o3d.geometry.AxisAlignedBoundingBox([-60, -60, -10], [60, 60, 20])
                pcd_box = self.point_cloud.crop(box)
                self.vis.update_geometry(self.point_cloud)
                if self.to_reset_view_point:
                    self.vis.reset_view_point(True)
                    self.to_reset_view_point = False
                self.vis.poll_events()
                self.vis.update_renderer()

    # =========================================相机=======================================================

    def put_camera_data(self, image):
        image_data = image.raw_data
        image_array = np.array(image_data)
        image_rgb = image_array.reshape((image.height, image.width, 4))
        self.png_queue.put(image_rgb)

    # 将相机图片保存到本地
    def save_show_camera_image(self):
        image_rgb = self.png_queue.get(True, 1.0)
        if self.camera_image_show:
            # 设置相机视频显示窗口
            cv2.namedWindow('Carla Camera', cv2.WINDOW_NORMAL)
            cv2.resizeWindow('Carla Camera', 600, 500)
            # 显示相机视频
            cv2.imshow('Carla Camera', image_rgb)
            # waitKey指定每帧显示时长，单位为毫秒
            cv2.waitKey(1)
        if self.camera_image_save:
            p_timestamp = self.world.get_snapshot().timestamp.platform_timestamp
            w_timestamp = self.get_time_stamp(p_timestamp)
            path = os.path.join(self.root_path, 'png')
            if not os.path.exists(path):
                os.makedirs(path)
            png_name = str(w_timestamp) + '.png'
            png_path = os.path.join(path,png_name )
            # 将Carla图像转换为OpenCV图像格式（BGR）
            image_bgr = cv2.cvtColor(image_rgb, cv2.COLOR_BGRA2RGB)
            # 保存图像
            try:
                cv2.imwrite(png_path, image_bgr)
                print(f"Image saved successfully: {png_path}")
            except Exception as e:
                print(f"Failed to save image: {e}")

    # 创建相机，并绑定
    def set_camera(self, carla_car):
        # 获取carla中的相机
        cam_bp = self.blueprint_library.find('sensor.camera.rgb')
        # 设置传感器采集图片的尺寸、水平翻转
        cam_bp.set_attribute("image_size_x", '600')
        cam_bp.set_attribute("image_size_y", '500')
        cam_bp.set_attribute("fov", '90')
        # 设置相机位置和旋转角度（相对车辆）
        camera_transform = carla.Transform(carla.Location(x=0, y=0, z=3), carla.Rotation(pitch=0, yaw=0, roll=0))
        # 将相机绑定到车上
        cam01 = self.world.spawn_actor(cam_bp, camera_transform, attach_to=carla_car)
        cam01.listen(lambda data: self.put_camera_data(data))
        self.camera_list.append(cam01)


if __name__ == "__main__":
    # myCarla = myCarla('127.0.0.1', 2000)
    myCarla = myCarla('10.100.16.98', 2000)
    executor = ThreadPoolExecutor(max_workers=3)

    myCarla.set_weather()
    myCarla.set_view(121.20237823425242, 31.291840026331244)
    myCarla.set_traffic_manager()
    # myCarla.set_car()
    # 接收并设置背景车辆信息
    # redis_get_bjc_channel = 'bjc_channel'
    # executor.submit(myCarla.set_bj_car_for_redis, redis_get_bjc_channel)
    # 接收并设置主车辆信息
    redis_get_target_channel = 'target_channel_1'
    executor.submit(myCarla.set_target_car_for_redis, redis_get_target_channel)

    # carla中车辆信息发送数据到kafka
    # redis_send_channel = 'carla_info'
    # executor.submit(myCarla.send_data_for_redis, redis_send_channel)
